#ifndef __CM5_H__
#define __CM5_H__

#include "dynamixel.h"

// libdarwin uses ID 200 for the CM-730
// since we want the CM-5 to act similarly define the controller ID here
#define CONTROLLER_ID                 200

// copy of CM730 address table used by libdarwin
enum
{
    CM5_MODEL_NUMBER_L		= 0,
    CM5_MODEL_NUMBER_H		= 1,
    CM5_VERSION				= 2,
    CM5_ID					= 3,
    CM5_BAUD_RATE			= 4,
    CM5_RETURN_DELAY_TIME	= 5,
    CM5_RETURN_LEVEL		= 16,
    CM5_DXL_POWER			= 24,
    CM5_LED_PANNEL			= 25,
    CM5_LED_HEAD_L			= 26,
    CM5_LED_HEAD_H			= 27,
    CM5_LED_EYE_L			= 28,
    CM5_LED_EYE_H			= 29,
    CM5_BUTTON				= 30,
    CM5_GYRO_Z_L			= 38,
    CM5_GYRO_Z_H			= 39,
    CM5_GYRO_Y_L			= 40,
    CM5_GYRO_Y_H			= 41,
    CM5_GYRO_X_L			= 42,
    CM5_GYRO_X_H			= 43,
    CM5_ACCEL_X_L			= 44,
    CM5_ACCEL_X_H			= 45,
    CM5_ACCEL_Y_L			= 46,
    CM5_ACCEL_Y_H			= 47,
    CM5_ACCEL_Z_L			= 48,
    CM5_ACCEL_Z_H			= 49,
    CM5_VOLTAGE				= 50,
    CM5_LEFT_MIC_L			= 51,
    CM5_LEFT_MIC_H			= 52,
    CM5_ADC2_L				= 53,
    CM5_ADC2_H				= 54,
    CM5_ADC3_L				= 55,
    CM5_ADC3_H				= 56,
    CM5_ADC4_L				= 57,
    CM5_ADC4_H				= 58,
    CM5_ADC5_L				= 59,
    CM5_ADC5_H				= 60,
    CM5_ADC6_L				= 61,
    CM5_ADC6_H				= 62,
    CM5_ADC7_L				= 63,
    CM5_ADC7_H				= 64,
    CM5_ADC8_L				= 65,
    CM5_ADC8_H				= 66,
    CM5_RIGHT_MIC_L			= 67,
    CM5_RIGHT_MIC_H			= 68,
    CM5_ADC10_L				= 69,
    CM5_ADC10_H				= 70,
    CM5_ADC11_L				= 71,
    CM5_ADC11_H				= 72,
    CM5_ADC12_L				= 73,
    CM5_ADC12_H				= 74,
    CM5_ADC13_L				= 75,
    CM5_ADC13_H				= 76,
    CM5_ADC14_L				= 77,
    CM5_ADC14_H				= 78,
    CM5_ADC15_L				= 79,
    CM5_ADC15_H				= 80,
    CM5_MAX_ADDRESS
};

extern uint8_t cm5_controlTable[CM5_MAX_ADDRESS];


// initialize the control table with default values
void initCM5(void);


// process a packet intended for the controller
// NOTE: we don't verify the ID here, so make sure the packet is actually meant for the CM-5!!
void processPacket(DXL_Packet *packet);


#endif // __CM5_H__
